In [25]:
# flight data src :
# https://conservancy.umn.edu/handle/11299/170139
from numpy import *
from matplotlib.pyplot import *
import matplotlib.animation as anm
from mpl_toolkits.mplot3d import Axes3D
import matplotlib as mpl
zoom=1.3
mpl.rcParams['figure.figsize'] = [6.4 *zoom , 4.8*zoom]
mpl.rcParams['figure.dpi'] = 80*zoom
mpl.rcParams['lines.linewidth'] = 2.0
ion()
ジャイロセンサデータの積分¶
- 角速度ベクトルをグローバル座標系に変換
- グローバル座標系で微小回転行列を生成
- 現在の推定値に微小回転行列を積算
In [26]:
def loaddata(fname,datarange):
d = loadtxt(fname,delimiter=",",skiprows=1+datarange[0],max_rows=datarange[1]).transpose()
sl = slice(0,d.shape[1])
t = (d[0,sl]).squeeze()
t = (t - t[0])/1000/1000 # sec
th = (d[8:11,sl]).squeeze() /10000 # rad
om = (d[11:14,sl]).squeeze() /10000 # rad/s
return (t,th,om)
def Zm1(data):
s = data.shape
Zdata = roll(data,(0,1),axis=(0,1))
Zdata[:,0] = zeros((s[0]))
return Zdata
def rpy2R(thx,thy,thz):
Rz = array([[cos(thz),-sin(thz),0],[sin(thz),cos(thz),0],[0,0,1]])
Ry = array([[cos(thy),0,sin(thy)],[0,1,0],[-sin(thy),0,cos(thy)]])
Rx = array([[1,0,0],[0,cos(thx),-sin(thx)],[0,sin(thx),cos(thx)]])
return Rz@Ry@Rx
A = lambda w : array([[0,-w[2],w[1]],[w[2],0,-w[0]],[-w[1],w[0],0]])
def infrot(w,R,dim=1):
Rot = np.eye(3)
Aw = A(w)
for i in range(dim):
k = i+1
Rot = Rot + (1/math.factorial(k))*Aw
Aw = Aw@Aw
return Rot@R
def R2rpy(R):
r = R
pitch = arcsin(-r[2,0])
sp = r[1,0]/cos(pitch)
cp = r[0,0]/cos(pitch)
yaw = arctan2(sp,cp)
sr = r[2,1]/cos(pitch)
cr = r[2,2]/cos(pitch)
roll = arctan2(sr,cr)
rpy =[roll,pitch,yaw]
return rpy
def matnorm(mat):
return trace(mat.transpose()@mat)
ion()
t_all,th_all,om_all= loaddata("flight_data.csv",[3000,6000])
dt_all = concatenate((zeros(1),diff(t_all)))
om_km1_all = Zm1(om_all)
Rref_all = [rpy2R(*th) for th in th_all.transpose()]
def estR(estimate_method):
Rlist = []
RPY = zeros(th_all.shape)
r31 = zeros(dt_all.shape)
Rnorm = zeros(dt_all.shape)
Rest_km1 = rpy2R(*th_all[:,0])
for i,(dt,om_km1) in enumerate(zip(dt_all, om_km1_all.transpose())):
Rest_k = estimate_method(om_km1,Rest_km1,dt,i)
rpy_est_k = R2rpy(Rest_k)
Rlist.append(Rest_k)
RPY[:,i] = rpy_est_k
r31[i] = Rest_k[2,0]
Rnorm[i] = matnorm(Rest_k)
Rest_km1 = Rest_k
err = RPY - th_all
ret = {
"R" : Rlist,
"rpy" : RPY,
"err" : err,
"r31" : r31,
"Rnorm" : Rnorm
}
return ret
def reference(om_km1,R_km1,dt,i):
return Rref_all[i]
def O1(om_km1,R_km1,dt,i):
om_in = R_km1 @ om_km1 * dt
R_k = infrot(om_in,R_km1,dim=1)
return R_k
def On(om_km1,R_km1,dt,i):
om_in = R_km1 @ om_km1 * dt
R_k = infrot(om_in,R_km1,dim=5)
return R_k
def O1_correct(om_km1,R_km1,dt,i):
om_in = R_km1 @ om_km1 * dt
R_tmp = infrot(om_in,R_km1,dim=1)
U,S,V = linalg.svd(R_tmp)
UV = U@V
UVdet = linalg.det(UV)
Shat = diag([1,1,UVdet])
R_k = U@ Shat @V
return R_k
cl = lambda x : x+"-"
ret ={
"Ref" : (estR(reference),cl("k")),
"O1" : (estR(O1),cl("r")),
"On" : (estR(On),cl("b")),
"O1c" : (estR(O1_correct),cl("m")),
}
def plot_ts(datalist,labels,linsps,yl,tl,newfig=True):
if newfig:
figure()
for d,l,c in zip(datalist,labels,linsps):
plot(t_all,d,c,label=l)
grid()
xlabel("time sec")
ylabel(yl)
title(tl)
legend()
show()
def plot_ts3(datalistsn,labels,linsps,yls3,tl):
figure()
for i in range(3):
subplot(3,1,i+1)
datalist = [data[i,:] for data in datalistsn]
plot_ts(datalist,labels,linsps,yls3[i],"",False)
suptitle(tl)
show()
def plotter(key2,key1list):
arg1 = [ret[key1][0][key2] for key1 in key1list]
arg2 = key1list
arg3 = [ret[key1][1] for key1 in key1list]
return arg1,arg2,arg3
def plotter_rpy(key1list):
arg1,arg2,arg3 = plotter("rpy",key1list)
arg1 = list(map(rad2deg,arg1))
arg4 = ["roll","pitch","yaw"]
arg5 = "time[sec] vs att[deg]"
return arg1,arg2,arg3,arg4,arg5
def plotter_err(key1list):
arg1,arg2,arg3 = plotter("err",key1list)
arg1 = list(map(rad2deg,arg1))
arg4 = ["roll","pitch","yaw"]
arg5 = "time[sec] vs atterr[deg]"
return arg1,arg2,arg3,arg4,arg5
arg = plotter_rpy(["Ref"])
arg[0].append(rad2deg(th_all))
arg[1].append("recalc")
arg[2].append("g--")
RPYアルゴリズム確認¶
RPY_ref -> R -> RPY_recalc を自前で計算する．
RPY_ref = RPY_recalcであることを確認して，考えているrpyの回転順番がデータセットと同じであることを確認する．
In [27]:
# basic rpy calc algo
plot_ts3(*arg)
1次の無限小回転¶
- time vs att : 累積誤差が見られる．積分アルゴリズムは合ってそう
- time vs err : 終盤rpyが計算できていない．
- r31 : rpy計算不可の理由は$\mathrm{pitch}=sin^{-1}r_{31}$が定義域外(-1以下,1以上)で計算結果がNaNになってるから
- Rnorm : その要因は回転行列が近似誤差によって厳密に直交行列じゃないから
行列が直交行列にどれくらい近いか，は以下のユークリッドノルムorフロベニウスノルムで与えられる． $$||\boldsymbol{R}||^2 = tr(\boldsymbol{RR}^\mathrm{T}) = tr(\boldsymbol{R}^\mathrm{T}\boldsymbol{R})$$ 直交行列に近いとき上記ノルムは$tr(\boldsymbol{E})=3$になる．
In [28]:
# evaluate O1
tgt = ["Ref","O1"]
plot_ts3(*plotter_rpy(tgt))
plot_ts3(*plotter_err(tgt))
plot_ts(*plotter("r31",tgt),"r31","time vs r31")
plot_ts(*plotter("Rnorm",tgt),"Rnorm","time vs Rnorm")
高次の無限小回転¶
今回5次で無限小回転を近似した．
- time vs rpy : 全区間においてRPYが定義できている．
- time vs err : 1次近似よりもrefに近づく．角速度が大きいとき(=微小回転が微小とみなせないとき)に誤差が大きくなる．
- r31 : 高次で近似したことによってr31は定義域内に収まる．
- Rnorm : 高次近似によって直交行列にかなり近い(縦軸スケールが1次とぜんぜん違う)が，時間と共にだんだん直交行列から離れてく．→そのうちRPYが定義できなくなると予想される．
In [29]:
# evaluate On
tgt = ["Ref","O1","On"]
tgt2 = ["Ref","On"]
tgt3 = ["O1","On"]
plot_ts3(*plotter_rpy(tgt))
plot_ts3(*plotter_err(tgt))
plot_ts(*plotter("r31",tgt3),"r31","time vs r31")
plot_ts(*plotter("Rnorm",tgt),"Rnorm","time vs Rnorm")
plot_ts(*plotter("Rnorm",tgt2),"Rnorm","time vs Rnorm")
1次の無限小回転 + 行列補正¶
$$\boldsymbol{\hat{R}} = U V\\ (\boldsymbol{R} = \boldsymbol{U\Sigma V}^\mathrm{T})$$- time vs rpy : 1次近似だけど高次近似とほぼ同じ精度の推定ができている．
- -
- -
- ノルムは常に3に等しいから誤差累積による定義域外の角度計算の恐れがない
In [30]:
# evaluate O1-correct
tgt = ["Ref","O1","O1c","On"]
tgt2 = ["Ref","On","O1c"]
tgt3 = ["Ref","O1c","O1"]
tgt4 = ["Ref","O1c"]
plot_ts3(*plotter_rpy(tgt))
plot_ts3(*plotter_err(tgt))
plot_ts(*plotter("r31",tgt3),"r31","time vs r31")
plot_ts(*plotter("Rnorm",tgt),"Rnorm","time vs Rnorm")
plot_ts(*plotter("Rnorm",tgt2),"Rnorm","time vs Rnorm")
plot_ts(*plotter("Rnorm",tgt4),"Rnorm","time vs Rnorm")
In [ ]:
In [ ]:
In [ ]:
In [ ]: